#include <stdio.h>
#include <string.h>
#include <stdint.h>
#include <stdbool.h>
#include <limits.h>
#include "config.h"
#include "common.h"
#include "radar_protocol.h"
#include "queue.h"
#include "platform.h"
#include "fusion.h"
#include "log.h"
#include "statistic.h"
#include "event.h"

radar_manager_t g_radar_manager = {0};

typedef struct {
    uint8_t type;
    char device_type[64];
} device_type_t;

static device_type_t device_type[] = {
    {RADAR_INCOMING, "来向雷达"},
    {RADAR_OUTGOING, "去向雷达"},
};

bool event_lane_filter(uint8_t lane_id)
{
    int i;
    for (i = 0; i < g_config.lane_num; i++) {
        if (g_config.lane_id[i] == lane_id) {
            return true;  // 车道号过滤通过
        }
    }
    return false;  // 车道号过滤不通过
}

void clac_coordinate_speed(radar_obj_info_t *radar_obj_info, Queue_t *radar_queue)
{
    uint8_t i, j;
    uint8_t frame_count = get_queue_count(radar_queue);
    radar_frame_t *radar_frame = NULL;
    double delta_longitude = 0, delta_latitude = 0;
    double speed_longitude = 0, speed_latitude = 0;
    int64_t current_time = get_current_time_in_milliseconds();
    int64_t delta_time = 0;
    for (i = 0; i < frame_count; i++) {
        radar_frame = (radar_frame_t *)queue_get_node(radar_queue, from_tail, i);
        if (radar_frame) {
            for (j = 0; j < radar_frame->obj_num; j++) {
                radar_obj_info_t *obj_info = &radar_frame->obj_info[j];
                if (obj_info->id == radar_obj_info->id) {
                    delta_longitude = radar_obj_info->longitude - obj_info->longitude;
                    delta_latitude = radar_obj_info->latitude - obj_info->latitude;
                    delta_time = current_time - radar_frame->frame_time;
                    break;
                }
            }
            if (j < radar_frame->obj_num) {
                break;
            }
        }
    }
    if (i == frame_count) {
        radar_obj_info->speed_latitude = 0.0f;
        radar_obj_info->speed_longitude = 0.0f;
    }
    else {
        radar_obj_info->speed_latitude = delta_latitude * 1000 / delta_time;
        radar_obj_info->speed_longitude = delta_longitude * 1000 / delta_time;
    }
}

bool is_false_target(uint16_t id)
{
    uint8_t i;
    uint8_t false_targets_count = get_queue_count(g_radar_manager.false_targets_queue);
    for (i = 0; i < false_targets_count; i++) {
        uint16_t *false_target_id = (uint16_t *)queue_get_node(g_radar_manager.false_targets_queue, from_head, i);
        if (false_target_id && *false_target_id == id) {
            return true;
        }
    }
    return false;
}

bool twin_pair_exists(uint16_t twin1, uint16_t twin2)
{
    int count = get_queue_count(g_radar_manager.twin_pair_queue);
    for (int i = 0; i < count; i++) {
        twin_pair_t *pair = (twin_pair_t *)queue_get_node(g_radar_manager.twin_pair_queue, from_head, i);
        if (pair && (pair->twin1 == twin1 && pair->twin2 == twin2)) {
            return true;
        }
    }
    return false;
}

void enqueue_twin_pair(uint16_t twin1, uint16_t twin2)
{
    // 保存前确保顺序为 twin1 <= twin2
    twin_pair_t pair = {0};
    if (twin1 <= twin2) {
        pair.twin1 = twin1;
        pair.twin2 = twin2;
    }
    else {
        pair.twin1 = twin2;
        pair.twin2 = twin1;
    }
    queue_enqueue(g_radar_manager.twin_pair_queue, &pair);
    printf("\033[0;33m保存孪生目标对: %04X, %04X\033[0m\n", pair.twin1, pair.twin2);
}

uint16_t find_twin_false_target(Queue_t *frame_queue, radar_obj_info_t *current_obj)
{
    const int MIN_FRAMES = 8;                  // 至少连续8帧满足条件
    const int DIST_THRESHOLD = 30 * 10;        // 30米范围（单位*10）
    const int DIST_ERROR_THRESHOLD = 1 * 10;   // 距离误差1米（单位*10）
    const int SPEED_ERROR_THRESHOLD = 2 * 10;  // 速度误差2米（单位*10）
    const int SPEED_DIFF_THRESHOLD = 15;       // 速度差小于1.5m/s，即15（单位*10）

    int consecutive_count = 0;
    int base_diff_distance = 0;
    int base_diff_speed = 0;
    bool firstValidFrame = true;
    uint16_t candidate_from_most_recent = 0;  // 最新帧得到的最终目标 id
    bool status = true;                       // 状态变量，初始为 true

    // 用于保存孪生目标对，按从小到大排序
    uint16_t twin1 = 0, twin2 = 0;

    int count = get_queue_count(frame_queue);
    if (count < MIN_FRAMES) {
        status = false;
    }
    else {
        // 从队列中从尾部开始查找（from_tail 为 true，下标0为最新帧）
        for (int i = 0; i < count; i++) {
            radar_frame_t *frame = queue_get_node(frame_queue, from_tail, i);
            if (frame == NULL) {
                status = false;
                break;
            }

            // 查找当前帧中与 current_obj->id 相同的基准目标
            radar_obj_info_t *base_obj = NULL;
            for (int j = 0; j < frame->obj_num; j++) {
                if (frame->obj_info[j].id == current_obj->id) {
                    base_obj = &frame->obj_info[j];
                    break;
                }
            }
            if (base_obj == NULL) {
                status = false;
                break;
            }

            // 如果基准目标已被标定为假目标，则退出
            if (is_false_target(base_obj->id)) {
                status = false;
                break;
            }

            // 构造待搜索车道的优先级列表：首先是当前车道，其次是左侧，再右侧
            int lanes_to_check[3];
            int num_lanes = 0;
            lanes_to_check[num_lanes++] = base_obj->lane;
            if (base_obj->lane - 1 >= 0) {
                lanes_to_check[num_lanes++] = base_obj->lane - 1;
            }
            if (base_obj->lane + 1 < g_config.lane_num) {
                lanes_to_check[num_lanes++] = base_obj->lane + 1;
            }

            radar_obj_info_t *raw_candidate = NULL;
            for (int k = 0; k < num_lanes && raw_candidate == NULL; k++) {
                int lane_to_check = lanes_to_check[k];
                for (int j = 0; j < frame->obj_num; j++) {
                    if (frame->obj_info[j].id == current_obj->id)
                        continue;
                    if (frame->obj_info[j].lane != lane_to_check)
                        continue;
                    int dist_diff = abs(frame->obj_info[j].dist_y - base_obj->dist_y);
                    int speed_diff = abs(frame->obj_info[j].speed - base_obj->speed);
                    if (dist_diff <= DIST_THRESHOLD && speed_diff <= SPEED_DIFF_THRESHOLD) {
                        raw_candidate = &frame->obj_info[j];
                        break;
                    }
                }
            }

            // 若候选目标不存在或已被标记为假目标，则退出
            if (raw_candidate == NULL || is_false_target(raw_candidate->id)) {
                status = false;
                break;
            }

            // 选择疑似假目标：这里修改为返回 id 较大的那个
            uint16_t effective_candidate = (raw_candidate->id > base_obj->id) ? raw_candidate->id : base_obj->id;

            // 在最新帧（i==0）时记录 candidate_from_most_recent 及孪生目标对
            if (i == 0) {
                candidate_from_most_recent = effective_candidate;
                // 保存孪生目标对：按从小到大排序
                uint16_t id1 = base_obj->id;
                uint16_t id2 = raw_candidate->id;
                if (id1 > id2) {
                    uint16_t tmp = id1;
                    id1 = id2;
                    id2 = tmp;
                }
                twin1 = id1;
                twin2 = id2;
            }
            else {
                // 后续帧中得到的目标 id 必须与最新帧记录的一致
                if (effective_candidate != candidate_from_most_recent) {
                    status = false;
                    break;
                }
            }

            // 计算当前帧中 base_obj 与 raw_candidate 的距离差和速度差（绝对值）
            int current_diff_distance = abs(raw_candidate->dist_y - base_obj->dist_y);
            int current_diff_speed = abs(raw_candidate->speed - base_obj->speed);
            if (firstValidFrame) {
                base_diff_distance = current_diff_distance;
                base_diff_speed = current_diff_speed;
                firstValidFrame = false;
            }
            else {
                if (abs(current_diff_distance - base_diff_distance) > DIST_ERROR_THRESHOLD ||
                    abs(current_diff_speed - base_diff_speed) > SPEED_ERROR_THRESHOLD) {
                    status = false;
                    break;
                }
            }
            consecutive_count++;
            if (consecutive_count >= MIN_FRAMES)
                break;
        }
    }

    // 如果找到了且状态有效，则尝试将这对孪生目标加入保存队列（如果队列中尚未存在该对）
    if (status && candidate_from_most_recent != 0) {
        if (!twin_pair_exists(twin1, twin2)) {
            enqueue_twin_pair(twin1, twin2);
        }
    }

    return status ? candidate_from_most_recent : 0;
}

bool update_new_enter_obj(radar_obj_info_t *radar_obj_info, int8_t dev_type)
{
    int8_t i;
    Queue_t *radar_queue = (dev_type == RADAR_INCOMING) ? g_radar_manager.in_new_obj_queue : g_radar_manager.out_new_obj_queue;
    int8_t entering_count = get_queue_count(radar_queue);
    for (i = 0; i < entering_count; i++) {
        relay_t *entering_obj = (relay_t *)queue_get_node(radar_queue, from_head, i);
        if (entering_obj && entering_obj->relay_id == radar_obj_info->id && entering_obj->update_status == update_normal) {
            entering_obj->update_time = get_current_time_in_milliseconds();
            memcpy(&entering_obj->radar_obj_info, radar_obj_info, sizeof(radar_obj_info_t));
            return false;
        }
    }

    char time_stamp[64] = {0};
    relay_t relay = {0};
    relay.relay_id = radar_obj_info->id;
    relay.update_time = get_current_time_in_milliseconds();
    relay.first_occurrence_pos_y = radar_obj_info->dist_y;
    relay.update_status = update_normal;
    memcpy(&relay.radar_obj_info, radar_obj_info, sizeof(radar_obj_info_t));
    queue_enqueue(radar_queue, &relay);
    return true;
}

int8_t lane_to_index(uint8_t lane)
{
    int8_t i;
    for (i = 0; i < g_config.lane_num; i++) {
        if (g_config.lane_id[i] == lane) {
            return i;
        }
    }
    return -1;
}

bool find_obj_by_id(radar_frame_t *radar_frame, uint16_t id)
{
    int i;
    for (i = 0; i < radar_frame->obj_num; i++) {
        if (radar_frame->obj_info[i].id == id) {
            return true;
        }
    }
    return false;
}

uint8_t count_last_frames(Queue_t *in_radar_frame_queue, uint16_t id)
{
    int8_t frame_count = get_queue_count(in_radar_frame_queue);
    uint8_t count = 0;
    for (int i = 0; i < frame_count; i++) {
        radar_frame_t *radar_frame = (radar_frame_t *)queue_get_node(in_radar_frame_queue, from_tail, i);
        if (radar_frame) {
            if (find_obj_by_id(radar_frame, id) == true) {
                count++;
            }
        }
    }
    return count;
}

int16_t get_obj_first_occurrence_pos(radar_obj_info_t *radar_obj_info, int8_t dev_type)
{
    int16_t first_occurrence_pos = radar_obj_info->dist_y;
    Queue_t *radar_queue = (dev_type == RADAR_INCOMING) ? g_radar_manager.in_new_obj_queue : g_radar_manager.out_new_obj_queue;
    int8_t entering_count = get_queue_count(radar_queue);
    for (int i = 0; i < entering_count; i++) {
        relay_t *entering_obj = (relay_t *)queue_get_node(radar_queue, from_head, i);
        if (entering_obj && entering_obj->relay_id == radar_obj_info->id) {
            first_occurrence_pos = entering_obj->first_occurrence_pos_y;
            break;
        }
    }
    return first_occurrence_pos;
}

bool filter_radar_obj(radar_obj_info_t *radar_obj_info, int8_t dev_type)
{
    if (dev_type == RADAR_INCOMING && get_obj_first_occurrence_pos(radar_obj_info, dev_type) < 1000) {
        // printf("\033[0;31m过滤掉来向雷达ID: 0x%04X, 道路号: %d, 距离: %d, 速度: %d\n\033[0m",
        //        radar_obj_info->id, radar_obj_info->lane, radar_obj_info->dist_y, radar_obj_info->speed_y);
        return true;
    }
    if (dev_type == RADAR_OUTGOING) {
        return false;
    }
    return false;
}

radar_obj_info_t *find_obj(const radar_frame_t *radar_frame, uint16_t id)
{
    for (uint16_t i = 0; i < radar_frame->obj_num; ++i) {
        if (radar_frame->obj_info[i].id == id) {
            return &radar_frame->obj_info[i];
        }
    }
    return NULL;
}

static void detect_and_resolve_overlap_generic(
    radar_frame_t *last_frame,
    const radar_frame_t *prev_frame,
    bool incoming)
{
    for (uint16_t i = 0; i < last_frame->obj_num; ++i) {
        radar_obj_info_t *li = &last_frame->obj_info[i];
        // 根据 incoming 选择初始化策略：
        // 来向：取 prev_frame 中 dist_y 最大的；去向：取最小的
        radar_obj_info_t *best_lj = NULL;
        int best_prev_dist = incoming ? INT_MIN : INT_MAX;

        for (uint16_t j = 0; j < last_frame->obj_num; ++j) {
            if (i == j)
                continue;
            radar_obj_info_t *lj = &last_frame->obj_info[j];
            if (li->lane != lj->lane)
                continue;

            radar_obj_info_t *pi = find_obj(prev_frame, li->id);
            radar_obj_info_t *pj = find_obj(prev_frame, lj->id);
            if (!pi || !pj)
                continue;

            if (incoming) {
                // 来向：上一帧 li 在后(pi > pj)、当前帧 li 在前(li <= lj)
                if (pi->dist_y > pj->dist_y && li->dist_y <= lj->dist_y) {
                    // 选取 prev_frame 中 dist_y 最大的那个 pj
                    if (pj->dist_y > best_prev_dist) {
                        best_prev_dist = pj->dist_y;
                        best_lj = lj;
                    }
                }
            }
            else {
                // 去向：上一帧 li 在前(pi < pj)、当前帧 li 在后(li >= lj)
                if (pi->dist_y < pj->dist_y && li->dist_y >= lj->dist_y) {
                    // 选取 prev_frame 中 dist_y 最小的那个 pj
                    if (pj->dist_y < best_prev_dist) {
                        best_prev_dist = pj->dist_y;
                        best_lj = lj;
                    }
                }
            }
        }

        if (best_lj) {
            // 再次查 pi/pj 仅用于日志
            radar_obj_info_t *pi = find_obj(prev_frame, li->id);
            radar_obj_info_t *pj = find_obj(prev_frame, best_lj->id);
            if (incoming) {
                if (g_config.event_debug == 1) {
                    printf("\033[0;33m来向雷达：目标 0x%04X 翻越 0x%04X，上一帧 Y=(%d, %d)，当前帧 Y=(%d, %d)\033[0m\n",
                           li->id, best_lj->id,
                           pi->dist_y, pj->dist_y,
                           li->dist_y, best_lj->dist_y);
                }
                li->dist_y = best_lj->dist_y + 50;
            }
            else {
                if (g_config.event_debug == 1) {
                    printf("\033[0;33m去向雷达：目标 0x%04X 翻越 0x%04X，上一帧 Y=(%d, %d)，当前帧 Y=(%d, %d)\033[0m\n",
                           li->id, best_lj->id,
                           pi->dist_y, pj->dist_y,
                           li->dist_y, best_lj->dist_y);
                }
                li->dist_y = best_lj->dist_y - 50;
            }
        }
    }
}

/**
 * 同时处理 in/out 两个雷达帧队列的重叠
 */
void process_manager_overlaps(radar_manager_t *mgr)
{
    // 处理来向队列
    radar_frame_t *last_in = (radar_frame_t *)queue_get_node(mgr->in_radar_frame_queue, from_tail, 0);
    radar_frame_t *prev_in = (radar_frame_t *)queue_get_node(mgr->in_radar_frame_queue, from_tail, 1);
    if (last_in && prev_in) {
        detect_and_resolve_overlap_generic(last_in, prev_in, true);
    }

    // 处理去向队列
    radar_frame_t *last_out = (radar_frame_t *)queue_get_node(mgr->out_radar_frame_queue, from_tail, 0);
    radar_frame_t *prev_out = (radar_frame_t *)queue_get_node(mgr->out_radar_frame_queue, from_tail, 1);
    if (last_out && prev_out) {
        detect_and_resolve_overlap_generic(last_out, prev_out, false);
    }
}

static void predicting_false_targets(radar_obj_info_t *radar_obj_info, int8_t dev_type)
{
    char buf[256] = {0};
    uint16_t false_target_id = 0;

    if (is_false_target(radar_obj_info->id))
        return;
}

void analysis_radar_buffer(char *buffer, int length, uint8_t dev_type)
{
    if (g_config.radar_type == RADAR_TYPE_BLG) {
        analysis_radar_buffer_blg(buffer, length, dev_type);
    }
    else if (g_config.radar_type == RADAR_TYPE_SSTK) {
        analysis_radar_buffer_sstk(buffer, length, dev_type);
    }
}

void radar_protocol_init(void)
{
    INIT_QUEUE(g_radar_manager.in_radar_frame_queue, RADAR_QUEUE_SIZE, sizeof(radar_frame_t), "in_radar_frame_queue");
    INIT_QUEUE(g_radar_manager.out_radar_frame_queue, RADAR_QUEUE_SIZE, sizeof(radar_frame_t), "out_radar_frame_queue");
    INIT_QUEUE(g_radar_manager.false_targets_queue, FALSE_TARGETS_QUEUE_SIZE, sizeof(uint16_t), "false_targets_queue");
    INIT_QUEUE(g_radar_manager.out_new_obj_queue, NEW_OBJ_QUEUE_SIZE, sizeof(relay_t), "out_new_obj_queue");
    INIT_QUEUE(g_radar_manager.in_new_obj_queue, NEW_OBJ_QUEUE_SIZE, sizeof(relay_t), "in_new_obj_queue");
    INIT_QUEUE(g_radar_manager.twin_pair_queue, TWIN_PAIR_QUEUE_SIZE, sizeof(twin_pair_t), "twin_pair_queue");
    if (g_config.radar_type == RADAR_TYPE_BLG) {
        INIT_QUEUE(g_radar_manager.event_static_queue, EVENT_STATIC_QUEUE_SIZE, sizeof(blg_event_static_t), "event_static_queue");
    }
    else if (g_config.radar_type == RADAR_TYPE_SSTK) {
        INIT_QUEUE(g_radar_manager.event_static_queue, EVENT_STATIC_QUEUE_SIZE, sizeof(sstk_event_static_t), "event_static_queue");
    }

    start_event_server();
}

void radar_protocol_deinit(void)
{
    SAFE_DESTROY_QUEUE(g_radar_manager.in_radar_frame_queue);
    SAFE_DESTROY_QUEUE(g_radar_manager.out_radar_frame_queue);
    SAFE_DESTROY_QUEUE(g_radar_manager.false_targets_queue);
    SAFE_DESTROY_QUEUE(g_radar_manager.out_new_obj_queue);
    SAFE_DESTROY_QUEUE(g_radar_manager.in_new_obj_queue);
    SAFE_DESTROY_QUEUE(g_radar_manager.twin_pair_queue);
    SAFE_DESTROY_QUEUE(g_radar_manager.event_static_queue);
    stop_event_server();
}

void print_frame_info(radar_frame_t *radar_frame)
{
    char timeinfo[64] = {0};
    printf("雷达数据包数据,时间: %s\n", get_current_time_string(timeinfo, sizeof(timeinfo)));
    for (int i = 0; i < radar_frame->obj_num; i++) {
        radar_obj_info_t *obj_info = &radar_frame->obj_info[i];
        printf("对象ID:0x%04X, 道路号: %d, 车辆类型: %s, 车辆长度: %d, 距离Y: %d, 距离X: %d, 速度Y: %d, 速度X: %d, 速度: %d, 经度: %f, 纬度: %f, 高度: %d\n",
               obj_info->id, obj_info->lane, get_car_type(0, obj_info->type), obj_info->length,
               obj_info->dist_y, obj_info->dist_x, obj_info->speed_y, obj_info->speed_x,
               obj_info->speed, obj_info->longitude, obj_info->latitude,
               obj_info->height);
    }

    // printf("obj_map:\n");
    // print_bitmap(&radar_frame->obj_map);
    // printf("--------------------\n");
}

void print_last_frame_info(void)
{
    int8_t i;
    radar_frame_t *radar_frame = NULL;
    int count = get_queue_count(g_radar_manager.in_radar_frame_queue);
    if (count == 0) {
        printf("没有雷达数据包\n");
        return;
    }
    for (i = 0; i < count; i++) {
        radar_frame = (radar_frame_t *)queue_get_node(g_radar_manager.in_radar_frame_queue, from_tail, i);
        if (radar_frame) {
            print_frame_info(radar_frame);
            break;
        }
    }
}

void test_pole_relay(struct_info_t *struct_info)
{
    static int count = 0;
    static int relay_success = 0;

    uint16_t id = get_radar_by_video(struct_info->guid, RADAR_INCOMING);
    if (id == 0) {
        // printf("没有找到雷达ID guid: %s\n", struct_info->guid);
        return;
    }
    else {
        printf("\033[38;5;220m结构化数据guid: %s,对应雷达id: 0x%04X\n\033[0m", struct_info->guid, id);
    }
    count++;

    relay_t *relay = get_successful_pole_relay_data(id);
    if (relay == NULL) {
        // printf("没有找到接力数据\n");
        return;
    }

    printf("\033[93m===========================================\n\033[0m");
    printf("接力ID:0x%04X, 车型: %s, 车身颜色:%s, 车牌颜色:%s, 车牌号码:%s\n",
           relay->relay_id, get_car_type(relay->video_obj_info.car_color, relay->video_obj_info.car_type),
           get_color_by_code(relay->video_obj_info.car_color),
           get_color_by_code(relay->video_obj_info.plate_color), relay->video_obj_info.plate_licence);

    printf("原生ID:0x%04X, 车型: %s, 车身颜色:%s, 车牌颜色:%s, 车牌号码:%s\n",
           id, get_car_type(struct_info->car_color, struct_info->car_type),
           get_color_by_code(struct_info->car_color),
           get_color_by_code(struct_info->plate_color), struct_info->plate_licence);

    if (relay->video_obj_info.car_color == struct_info->car_color || relay->video_obj_info.car_type == struct_info->car_type) {
        relay_success++;
    }
    if (relay->video_obj_info.car_color == 0) {
        relay_success++;
    }
    printf("\033[32m匹配成功数: %d, 总接力数: %d,匹配成功率: %0.2f%%\033[0m\n", relay_success, count, (float)relay_success / count * 100);

    printf("\033[93m===========================================\n\033[0m");
}